#include "swansee.h"
#include "renderer.h"
#include "input.h"

Swansee::Swansee(){

name="ragDoll";
drawType=DRAW_CUBE;

bFullBright=false;

//standard ragdoll...
UPPER_ARM_LEN = 0.30;
FORE_ARM_LEN = 0.25;
HAND_LEN = 0.13; // wrist to mid-fingers only
FOOT_LEN = 0.18; // ankles to base of ball of foot only
HEEL_LEN = 0.05;

BROW_H = 1.68;
MOUTH_H = 1.53;
NECK_H = 1.50;
SHOULDER_H = 1.37;
CHEST_H = 1.35;
HIP_H = 0.86;
KNEE_H = 0.48;
ANKLE_H = 0.08;

SHOULDER_W = 0.41;
CHEST_W = 0.36; // actually wider, but we want narrower than shoulders (esp. with large radius)
LEG_W = 0.28; // between middles of upper legs
PELVIS_W = 0.25; // actually wider, but we want smaller than hip width

activeFoot="none";
lftHandRel=Vector3f(0,0,0);             //relative to the Ragdoll's body - can this be abstracted?
rtHandRel=Vector3f(0,0,0);
lftFootRel=Vector3f(0,0,0);
rtFootRel=Vector3f(0,0,0);

lftFootRelToLast=Vector3f(0,0,0);       //relative to the last step
rtFootRelToLast=Vector3f(0,0,0);
lftFootLastAbsolute=Vector3f(0,0,0);    //last absolute foot position
rtFootLastAbsolute=Vector3f(0,0,0);
loffset=Vector3f(0,0,0);                //current left foot offset from the ragdoll's center in x-Axis
roffset=Vector3f(0,0,0);                //current right foot offset from the ragdoll's center in x-Axis
walkDistance=Vector3f(0,0,0);           //distance from the initial Swansee Position
spineAbsolute=Vector3f(0,1.55,0);             //the absolute position of the ragdoll's spine


bFlipFeet=false;              //flips control direction of feet
bUDPControlled=true;

baseOffset=0.02f;
//actionSpaceMultiplier=7;
actionSpaceMultiplier=7;

totalMass=0.0f;

registerProperties();
}


//TODO:register properties for all the strings!!!

Swansee::~Swansee(){

//clean up all our stuff!
map<string,dJoint*>::iterator it;
map<string, PhysicsActor*>::iterator p;

for ( it=joints.begin() ; it != joints.end(); it++ )
    dJointDestroy((*it).second->id());

for ( p=parts.begin() ; p != parts.end(); p++ )
    dBodyDestroy((*p).second->body);
}

void Swansee::registerProperties(){

createMemberID("LFTHANDREL", &lftHandRel, this);
createMemberID("LFTFOOTREL", &lftFootRel, this);
createMemberID("RTHANDREL", &rtHandRel, this);
createMemberID("RTFOOTREL", &rtFootRel, this);
}


void Swansee::setup(){
Actor::setup();

cout << "setting up..." << endl;
createSwansee();
}

void Swansee::createSwansee(){

R_SHOULDER_POS = Vector3f(-SHOULDER_W * 0.5, SHOULDER_H, 0.0);
L_SHOULDER_POS = Vector3f(SHOULDER_W * 0.5, SHOULDER_H, 0.0);
R_ELBOW_POS = R_SHOULDER_POS - Vector3f(UPPER_ARM_LEN, 0.0, 0.0);
L_ELBOW_POS = L_SHOULDER_POS + Vector3f(UPPER_ARM_LEN, 0.0, 0.0);
R_WRIST_POS = R_ELBOW_POS - Vector3f(FORE_ARM_LEN, 0.0, 0.0);
L_WRIST_POS = L_ELBOW_POS + Vector3f(FORE_ARM_LEN, 0.0, 0.0);
R_FINGERS_POS = R_WRIST_POS - Vector3f(HAND_LEN, 0.0, 0.0);
L_FINGERS_POS = L_WRIST_POS + Vector3f(HAND_LEN, 0.0, 0.0);

R_HIP_POS = Vector3f(-LEG_W * 0.5, HIP_H, 0.0);
L_HIP_POS = Vector3f(LEG_W * 0.5, HIP_H, 0.0);
R_KNEE_POS = Vector3f(-LEG_W * 0.5, KNEE_H, 0.0);
L_KNEE_POS = Vector3f(LEG_W * 0.5, KNEE_H, 0.0);
R_ANKLE_POS = Vector3f(-LEG_W * 0.5, ANKLE_H, 0.0);
L_ANKLE_POS = Vector3f(LEG_W * 0.5, ANKLE_H, 0.0);

R_HEEL_POS = R_ANKLE_POS - Vector3f(0.0, 0.0, HEEL_LEN);
L_HEEL_POS = L_ANKLE_POS - Vector3f(0.0, 0.0, HEEL_LEN);
R_TOES_POS = R_ANKLE_POS + Vector3f(0.0, 0.0, FOOT_LEN);
L_TOES_POS = L_ANKLE_POS + Vector3f(0.0, 0.0, FOOT_LEN);


Vector3f rightAxis = Vector3f(1.0, 0.0, 0.0);
Vector3f leftAxis = Vector3f(-1.0, 0.0, 0.0);
Vector3f upAxis = Vector3f(0.0, 1.0, 0.0);
Vector3f downAxis = Vector3f(0.0, -1.0, 0.0);
Vector3f bkwdAxis = Vector3f(0.0, 0.0, 1.0);
Vector3f fwdAxis = Vector3f(0.0, 0.0, -1.0);

float legWidth=0.03;

//create and setup parts

addBody(Vector3f(0.0, CHEST_H - 0.1, 0.0), Vector3f(0.0, HIP_H + 0.1, 0.0), 0.125, "belly");

addBody(Vector3f(-PELVIS_W * 0.5, HIP_H, 0.0),Vector3f(PELVIS_W * 0.5, HIP_H, 0.0), 0.125,"pelvis");
addBody(Vector3f(0.0, BROW_H, 0.0), Vector3f(0.0, MOUTH_H, 0.0), 0.11,"head");

addFixedJoint(parts["belly"]->body, parts["head"]->body,"neck");

//addHingeJoint(parts["belly"]->body, parts["pelvis"]->body, parts["chest"]->location, leftAxis, "lowSpine", 0.0, M_PI * 0.75);
addFixedJoint(parts["belly"]->body, parts["pelvis"]->body,"lowSpine");

//addHingeJoint(parts["rightUpperLeg"]->body, parts["rightLowerLeg"]->body, R_KNEE_POS, leftAxis, "rightKnee", 0.0, M_PI * 0.75);



addBody(R_HIP_POS, R_KNEE_POS, legWidth,"rightUpperLeg");
addUniversalJoint(parts["pelvis"]->body, parts["rightUpperLeg"]->body,R_HIP_POS, fwdAxis, rightAxis,"rightHip", -0.1 * M_PI, 0.3 * M_PI, -0.15 * M_PI, 0.75 * M_PI);
//addBallJoint(parts["pelvis"]->body, parts["rightUpperLeg"]->body,R_HIP_POS,"rightHip");

addBody(R_KNEE_POS, R_ANKLE_POS, legWidth, "rightLowerLeg");
addBallJoint(parts["rightUpperLeg"]->body, parts["rightLowerLeg"]->body, R_KNEE_POS,"rightKnee");
//addBallJoint(parts["rightUpperLeg"]->body, parts["rightLowerLeg"]->body, R_KNEE_POS,"rightKnee");

addBody(L_HIP_POS, L_KNEE_POS, legWidth,"leftUpperLeg");
addUniversalJoint(parts["pelvis"]->body, parts["leftUpperLeg"]->body,L_HIP_POS, bkwdAxis, rightAxis,"leftHip", -0.1 * M_PI, 0.3 * M_PI, -0.15 * M_PI,0.75 * M_PI);
//addBallJoint(parts["pelvis"]->body, parts["leftUpperLeg"]->body,L_HIP_POS,"leftHip");

addBody(L_KNEE_POS, L_ANKLE_POS, legWidth, "leftLowerLeg");
addBallJoint(parts["leftUpperLeg"]->body, parts["leftLowerLeg"]->body, L_KNEE_POS, "leftKnee");
//addBallJoint(parts["leftUpperLeg"]->body, parts["leftLowerLeg"]->body, L_KNEE_POS,"leftKnee");

addBody(R_HIP_POS+Vector3f(0.15,0,0), R_KNEE_POS+Vector3f(0.15,0.0,0.0), legWidth,"thirdUpperLeg");
addUniversalJoint(parts["pelvis"]->body, parts["thirdUpperLeg"]->body,R_HIP_POS+Vector3f(0.15,0,0), fwdAxis, rightAxis,"thirdHip", -0.1 * M_PI, 0.3 * M_PI, -0.15 * M_PI, 0.75 * M_PI);
//addBallJoint(parts["pelvis"]->body, parts["rightUpperLeg"]->body,R_HIP_POS,"rightHip");

addBody(R_KNEE_POS+Vector3f(0.15,0.0,0.0), R_ANKLE_POS+Vector3f(0.15,0.0,0.1), legWidth, "thirdLowerLeg");
addBallJoint(parts["thirdUpperLeg"]->body, parts["thirdLowerLeg"]->body, R_KNEE_POS+Vector3f(0.15,0.0,0.0), "thirdKnee");
//addBallJoint(parts["rightUpperLeg"]->body, parts["rightLowerLeg"]->body, R_KNEE_POS,"rightKnee");

//*****************************************************************************************************

//     S T R I N G S

//*****************************************************************************************************

//spine
Vector3f spinePos=Vector3f(0.0, CHEST_H, 0.0);

addString(spinePos+Vector3f(0,0,-0.15), spinePos+Vector3f(0,0,-0.3), 0.015,"spineString");  //original!
addFixedJoint(parts["belly"]->body, myStrings["spineString"]->body,"spineStringConnect");

//addBallJoint(parts["chest"]->body, myStrings["spineString"]->body,spinePos+Vector3f(0,0,0.15),"spineStringConnect");
//myStrings["spineString"]=parts["chest"];
createStringJoint("spineString");

}


void Swansee::update(double deltaTime){

Actor::update(deltaTime);
updateString("spineString",myStrings["spineString"]->location);
}

void Swansee::drawCube(){

    glPushMatrix();

    //don't need to, because all the individual parts are doing this!!!
    //glTranslatef(location.x,location.y,location.z);

    //don't need to, because all the individual parts are doing this!!!
    //glRotatef(rotation.x,1,0,0);
    //glRotatef(rotation.y,0,1,0);
    //glRotatef(rotation.z,0,0,1);

    std::map <std::string, PhysicsActor*>::iterator it;


    for ( it=parts.begin() ; it != parts.end(); it++ )
        {
          PhysicsActor * pA = it->second;
          pA->color=color;
          pA->draw();
        }
    glPopMatrix();
}

void Swansee::drawCollision(){

    glPushMatrix();

    if (base)
        glLoadMatrixf(base->baseMatrix);

    //don't need to, because all the individual parts are doing this!!!
    if (bUseTransformMatrix)
        glMultMatrixf(transformMatrix);
    else{
        glTranslatef(location.x,location.y,location.z);
        glRotatef(rotation.x,1,0,0);
        glRotatef(rotation.y,0,1,0);
        glRotatef(rotation.z,0,0,1);
        }

    std::map <std::string, PhysicsActor*>::iterator it;

    //added for picking, so we know about sub-objects
    for ( it=parts.begin() ; it != parts.end(); it++ )
        {
          PhysicsActor * pA = it->second;
          pA->color=color;
          pA->draw();
        }

//these are helpers now...
/*
    for ( it=myStrings.begin() ; it != myStrings.end(); it++ )
        {
          PhysicsActor * pA = it->second;
          pA->color=color;
          glPushName(STRINGS);
          glPushName(pickName++);
          pA->draw();
          glPopName();
          glPopName();
        }
*/
    glPopMatrix();
}


void Swansee::addBody(Vector3f p1, Vector3f p2, float radius, string partName){

		PhysicsActor * newPart = new PhysicsActor();

        float length=(p2-p1).length();

		newPart->shape.x=radius;
		newPart->shape.y=length;

        newPart->setup();
        newPart->name=partName;

		Vector3f myLoc;
        myLoc=(p1+p2)*0.5;

		newPart->setLocation(myLoc+location);
		newPart->setRotationODE(p2-p1);

		totalMass += newPart->mass;

        newPart->parent=this;

        parts[partName]=newPart;
}


void Swansee::addString(Vector3f p1, Vector3f p2, float radius, string partName){

		PhysicsActor * newPart = new PhysicsActor();

        Vector3f myLoc=p1;

        float length=(p2-p1).length();

		newPart->shape.x=radius;
		newPart->shape.y=length;

        newPart->setup();
        newPart->name=partName;

		newPart->setLocation(myLoc+location);

		newPart->setRotationODE(p2-p1);

        newPart->parent=this;

        myStrings[partName]=newPart;

        newPart->base=this;

        renderer->helperList.push_back(newPart);
}


void Swansee::addFixedJoint(dBodyID body1,dBodyID body2, string name){

        dFixedJoint * joint = new dFixedJoint(renderer->physicsWorld);

		joint->attach(body1, body2);
		joint->set();

		joints[name]=joint;
}

void Swansee::addBallJoint(dBodyID body1, dBodyID body2, Vector3f anchor, string name){

        anchor+=location;
        // create the joint
		dBallJoint * joint = new dBallJoint(renderer->physicsWorld);

		joint->attach(body1, body2);
		joint->setAnchor(anchor.x,anchor.y,anchor.z);

        joints[name]=joint;
}

void Swansee::addHingeJoint( dBodyID body1, dBodyID body2, Vector3f anchor,Vector3f axis, string name, float loStop, float hiStop){
		anchor +=location;

		dHingeJoint * joint = new dHingeJoint(renderer->physicsWorld);

		joint->attach(body1, body2);
		joint->setAnchor(anchor.x,anchor.y,anchor.z);
		joint->setAxis(axis.x,axis.y,axis.z);

        joint->setParam(dParamLoStop, loStop);
		joint->setParam(dParamHiStop, hiStop);
        joints[name]=joint;
}

void Swansee::addUniversalJoint(dBodyID body1,dBodyID body2,Vector3f anchor,Vector3f axis1,Vector3f axis2, string name,float loStop1, float hiStop1, float loStop2, float hiStop2){

		anchor +=location;

		dUniversalJoint * joint = new dUniversalJoint(renderer->physicsWorld);
		joint->attach(body1, body2);
		joint->setAnchor(anchor.x,anchor.y,anchor.z);
		joint->setAxis1(axis1.x,axis1.y,axis1.z);
		joint->setAxis2(axis2.x,axis2.y,axis2.z);
		joint->setParam(dParamLoStop, loStop1);
		joint->setParam(dParamHiStop, hiStop1);
		joint->setParam(dParamLoStop2, loStop2);
		joint->setParam(dParamHiStop2, hiStop2);

		joints[name]=joint;
}




void Swansee::trigger(Actor * other){

}


void Swansee::createStringJoint(string stringName){

//let's do this manually...
		dFixedJoint * joint = new dFixedJoint(renderer->physicsWorld);

		joint->attach(0,myStrings[stringName]->body);
		joint->set();

        joints[stringName]=joint;
}

void Swansee::updateString(string stringName, Vector3f loc){

dJointDestroy(joints[stringName]->id());
dBodySetPosition(myStrings[stringName]->body,loc.x,loc.y,loc.z);
myStrings[stringName]->location=loc;
createStringJoint(stringName);
}

void Swansee::reset(){

cout << "resetting...";
}

void Swansee::create(){
renderer->addActor(this);
std::cout << "creating a ragdoll! \n";
}

TiXmlElement* Swansee::save(TiXmlElement *root){

    return(Actor::save(root));
}

void Swansee::load(TiXmlElement *myInfo){

    Actor::load(myInfo);
}
